import time,sys,os,utime
from media.sensor import *
from media.display import *
from media.media import *
import image
from machine import UART
from machine import FPIOA

#初始化UART
fpioa=FPIOA()

fpioa.set_function(11,FPIOA.UART2_TXD)
fpioa.set_function(12,FPIOA.UART2_RXD)

uart1=UART(2,baudrate=115200,bits=UART.EIGHTBITS,parity=UART.PARITY_NONE,stop=UART.STOPBITS_ONE)

#蓝色块区间
blue_threshold=(0, 38, -128, 127, -128, -24)
#中心点阈值
threshold=30

send1=""
send2=""

sensor_id=2

# 显示模式选择：可以是 "VIRT"、"LCD" 或 "HDMI"
DISPLAY_MODE = "VIRT"

# 根据模式设置显示宽高
if DISPLAY_MODE == "VIRT":
    # 虚拟显示器模式
    DISPLAY_WIDTH = ALIGN_UP(1920, 16)
    DISPLAY_HEIGHT = 1080
elif DISPLAY_MODE == "LCD":
    # 3.1寸屏幕模式
    DISPLAY_WIDTH = 800
    DISPLAY_HEIGHT = 480
elif DISPLAY_MODE == "HDMI":
    # HDMI扩展板模式
    DISPLAY_WIDTH = 1920
    DISPLAY_HEIGHT = 1080
else:
    raise ValueError("未知的 DISPLAY_MODE，请选择 'VIRT', 'LCD' 或 'HDMI'")

def Display_Init():
    if DISPLAY_MODE == "VIRT":
        Display.init(Display.VIRT, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, fps=60)
    elif DISPLAY_MODE == "LCD":
        Display.init(Display.ST7701,width=DISPLAY_WIDTH,height=DISPLAY_HEIGHT,to_ide=False)
    elif DISPLAY_MODE == "HDMI":
        Display.init(Display.LT9611, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True)
    else:
        raise ValueError("未知的 DISPLAY_MODE，请选择 'VIRT', 'LCD' 或 'HDMI'")

#寻找最大色块
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob.pixels()>max_size:
            max_blob=blob
            max_size=blob.pixels()
    return max_blob

try:
    #传感器初始化
    sensor=Sensor(id=sensor_id,width=DISPLAY_WIDTH,height=DISPLAY_HEIGHT)
    #复位
    sensor.reset()

    #设置输出尺寸和通道
    sensor.set_framesize(width=DISPLAY_WIDTH,height=DISPLAY_HEIGHT,chn=CAM_CHN_ID_1)
    #设置输出画面格式
    sensor.set_pixformat(Sensor.RGB565,CAM_CHN_ID_1)
    #是否水平翻转
    sensor.set_hmirror(False)
    #是否垂直翻转
    sensor.set_vflip(False)

    image_center_x=DISPLAY_WIDTH//2
    image_center_y=DISPLAY_HEIGHT//2

#    #关闭自动增益
#    sensor.set_auto_gain(False)
#    #关闭自动白屏
#    sensor.set_auto_whitebal(False)

    #显示设备初始化
    Display_Init()

    #媒体堆栈初始化
    MediaManager.init()

    sensor.run()

    #帧率显示对象初始化
    clock=utime.clock()

    while True:
        os.exitpoint()

        #更新当前时间
        clock.tick()

        #截取当前画面
        img=sensor.snapshot(CAM_CHN_ID_1)
        img.draw_cross(image_center_x,image_center_y,color=(255,0,0),size=25,thickness=6)
        blobs=img.find_blobs([blue_threshold],area_threshold=20000)
        if blobs:
            max_blob=find_max(blobs)
            blob_cx=max_blob.cx()
            blob_cy=max_blob.cy()
#            print("image_center_x:{},image_center_y:{}".format(max_blob.h()*max_blob.w(),blob_cy))
#            img.draw_rectangle(max_blob.x(),max_blob.y(),max_blob.w(),max_blob.h(),
#                               color=(0,255,0),thickness=2)
            img.draw_circle(blob_cx,blob_cy,max(max_blob.h(),max_blob.w())//2,
                            color=(0,255,0),thickness=3)

            delta_x=blob_cx-image_center_x
            delta_y=blob_cy-image_center_y


            if delta_x>80:
                send1='a'
            elif delta_x>30:
                send1='b'
            elif delta_x<-80:
                send1='c'
            elif delta_x<-30:
                send1='d'
            else:
                send1='o'

            if delta_y>80:
                send2='a'
            elif delta_y>30:
                send2='b'
            elif delta_y<-80:
                send2='c'
            elif delta_y<-30:
                send2='d'
            else:
                send1='o'

            print("send1:{},send2:{}\r\n".format(send1,send2))
            uart1.write("send1:{},send2:{}\r\n".format(send1,send2))
        else:
            uart1.write("Not Found")
            print("Not Found")



        #显示当前画面
        Display.show_image(img)

        print("fps: ",clock.fps())

except KeyboardInterrupt as e:
    print("用户停止: ", e)
except BaseException as e:
    print(f"异常: {e}")
finally:
    # 停止传感器运行
    if isinstance(sensor, Sensor):
        sensor.stop()
    # 反初始化显示模块
    Display.deinit()
    os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
    time.sleep_ms(100)
    # 释放媒体缓冲区
    MediaManager.deinit()

